package nape.space;
import cx.Allocator;
import cx.Algorithm;
import cx.FastList;
import nape.geom.VecMath;
import nape.geom.AABB;
import nape.geom.Vec2;
import nape.geom.Ray;
import nape.geom.RayResult;
import nape.phys.Body;
import nape.phys.PhysObj;
import nape.phys.Particle;
import nape.phys.Properties;
import nape.phys.Group;
import nape.callbacks.Callbackable;
import nape.callbacks.Callback;
import nape.shape.Shape;
import nape.dynamics.Arbiter;
import nape.dynamics.ObjArb;
import nape.dynamics.GroupArb;
import nape.dynamics.Collide;
import nape.dynamics.SubArbiters;
import nape.constraint.Constraint;
import nape.util.IdRef;
import nape.util.FastMath;
import nape.util.NIntHash;
import nape.Const;
import nape.Config;
import nape.space.UniformSleepSpace;





































class Space {
	
	vec_new(public,gravity);
	
	public var statics  :CxFastList(Body);
	public var dynamics :CxFastList(Body);
	public var particles:CxFastList(Particle);
	
	public var objects:CxFastList(PhysObj); 
	
	public var visible:CxFastList(PhysObj);
	public var viewport:AABB;
	public var use_viewport:Bool;
	
	
	
	public var STATIC:Body;
	public var constraints:CxFastList(Constraint);
	public var groups:CxFastList(Group);
		
	
	public var map_objarb  :IntHash<ObjArb>;
	public var map_grouparb:IntHash<GroupArb>;
	
	public var properties:CxFastList(Properties);
	
	public var stamp:Int;
	public var sleeping:Bool;
	
	public var alloc:Allocator;
	public var collide:Collide;
	
	public var callbacks:CxFastAllocList(Callback);
	public var register:Bool;
	
	public var _dt:Float; 
	
	
	
	public function new(?G:Vec2) {
		FastMath.init();
		
		alloc   = Allocator.GLOBAL;
		collide = new Collide(alloc);
		stamp   = 0;
		
		visible = new CxFastList(PhysObj)(alloc);
		viewport = new AABB(-Const.POSINF,-Const.POSINF,Const.POSINF,Const.POSINF);
		
		if(G==null) vec_set(gravity,0,0) else vec_set(gravity,G.p);
		
		
		map_objarb   = new IntHash<ObjArb>  ();
		map_grouparb = new IntHash<GroupArb>();
		
		constraints = new CxFastList(Constraint)(alloc);
		properties  = new CxFastList(Properties)(alloc);
		
		statics   = new CxFastList(Body)    (alloc);
		dynamics  = new CxFastList(Body)    (alloc);
		particles = new CxFastList(Particle)(alloc);
		objects   = new CxFastList(PhysObj) (alloc);
		groups    = new CxFastList(Group)   (alloc);
		
		STATIC = new Body(0,0);
		STATIC.imass   = STATIC.smass  = STATIC.imoment = STATIC.smoment = STATIC.cmass = STATIC.cmoment = 0;
		STATIC.mass    = STATIC.moment = Const.POSINF;
		STATIC.rotx    = 0; STATIC.roty = 1;
		STATIC.kinetic = 0;
		STATIC.sleep   = true;
		STATIC.added_to_space = true;
		
		 cb_Begin = new IntHash<Bool>();  cb_End = new IntHash<Bool>();  cb_SenseBegin = new IntHash<Bool>();  cb_SenseEnd = new IntHash<Bool>();  cb_PostSolve = new IntHash<Bool>(); 
		cb_PreSolve = new IntHash<Arbiter->Int>();
		cb_PreBegin = new IntHash<Arbiter->Int>();
		
		callbacks = new CxFastAllocList(Callback)(alloc);
	}
	
	
	
	public inline function addProperties(p:Properties) {
		p.count++;
		if(p.count==1) properties.add(p);
	}
	public inline function removeProperties(p:Properties) {
		p.count--;
		if(p.count <= 0) properties.remove(p);
	}
	
	
	
	inline function add_aux(o:PhysObj) o.space = this
	inline function rem_aux(o:PhysObj) o.space = null
	
	

	public inline function addConstraint(c:Constraint) {
		if(!c.live) {
			c.live = true;
			
			if(!constraints.has(c)) {
				constraints.add(c);
				c.addToBodies();
			}
			c.wakeBodies(this);
		}
	}
	public inline function removeConstraint(c:Constraint) {
		c.removeFromBodies();
		c.live = false;
		constraints.remove(c);
		c.wakeBodies(this);
	}
	
	public inline function removeConstraints(o:PhysObj) {
		var cons = o.p_constraints;
		while(!cons.empty()) {
			var c = cons.front();
			cons.pop();
			removeConstraint(c);
		}
	}
	
	public function wakeConstraint(c:Constraint) {
		if(c.canSleep || c.sleep) {
			c.canSleep = c.sleep = false;
			if(!c.live) {
				c.live = true;
				constraints.add(c);
			}
			c.wakeBodies(this);			
		}
	}
	
	
	
	
	public inline function addStatic  (b:Body)     statics  .add(b)
	public inline function addDynamic (b:Body)     dynamics .add(b)
	public inline function addParticle(p:Particle) particles.add(p)
	
	public inline function removeStatic  (b:Body)     return statics  .remove(b)
	public inline function removeDynamic (b:Body)     return dynamics .remove(b)
	public inline function removeParticle(p:Particle) return particles.remove(p)

	
	
	public function clear_special();
	public inline function clear() {
		while(!objects.empty()) {
			var obj = objects.front();
			obj.added_to_space = false;
			if(obj.graphic!=null) {
				if(obj.graphic.parent!=null && obj.graphic.parent.contains(obj.graphic))
					obj.graphic.parent.removeChild(obj.graphic);
			}
			objects.pop();
			removeObject(obj);
		}
		while(!constraints.empty()) {
			var obj = constraints.front();
			removeConstraint(obj);
			constraints.pop();
		}
		while(!groups.empty()) {
			var grp = groups.front();
			removeGroup(grp);
			groups.pop();
		}
		
		visible.clear();
		
		
		
		map_objarb   = new IntHash<ObjArb>  ();
		map_grouparb = new IntHash<GroupArb>();
		
		properties.clear();
		statics   .clear();
		dynamics  .clear();
		particles .clear();
		callbacks .clear();
		
		 cb_Begin = new IntHash<Bool>(); cb_End = new IntHash<Bool>(); cb_SenseBegin = new IntHash<Bool>(); cb_SenseEnd = new IntHash<Bool>(); cb_PostSolve = new IntHash<Bool>();
		cb_PreSolve = new IntHash<Arbiter->Int>();
		cb_PreBegin = new IntHash<Arbiter->Int>();
		
		clear_special();
		
		stamp++;
	}
	
	
	
	public inline function updateProp(dt:Float) {
		CxIter(p,properties,{
			p.lfdt = Math.pow(p.linDamp, dt);
			p.afdt = Math.pow(p.angDamp, dt);
		});
	}
	
	public inline function updateVel_remSleep(dt:Float) {
		
		
		 {
			var pre = null;
			CxIter(cur,dynamics,{
				if(cur.stamp<stamp && sleeping) {
					cxiterator = dynamics.erase(pre,cxiterator);
					cur.live = false;
					 {
	if(cur.cbSleep) {
		var cb = alloc.CxAlloc(Callback)();
		cb.type = Callback.PHYSOBJ_SLEEP;
		cb.obj = cur;
		callbacks.add(cb);
	}else if(cur.cbSleepDef)
		Config.DEFAULT_CB_SLEEP(cur);
};
					cur.plive = false;
					removeProperties(cur.properties);
					continue;
				}else if(sleeping && cur.plive) {
					 {
	if(cur.cbWake) {
		var cb = alloc.CxAlloc(Callback)();
		cb.type = Callback.PHYSOBJ_WAKE;
		cb.obj = cur;
		callbacks.add(cb);
	}else if(cur.cbWakeDef)
		Config.DEFAULT_CB_WAKE(cur);
};
					addProperties(cur.properties);
				}
				cur.plive = false;
				vec_addeq(cur.f,gravity,cur.gmass);
				if(cur.isBody) {
					var body = cur.body;
					body.t += vec_cross(body.gp,gravity)*body.gmass;
				}
				cur.updateVelocity(dt);
				pre = cxiterator;
			});
		};
		 {
			var pre = null;
			CxIter(cur,particles,{
				if(cur.stamp<stamp && sleeping) {
					cxiterator = particles.erase(pre,cxiterator);
					cur.live = false;
					 {
	if(cur.cbSleep) {
		var cb = alloc.CxAlloc(Callback)();
		cb.type = Callback.PHYSOBJ_SLEEP;
		cb.obj = cur;
		callbacks.add(cb);
	}else if(cur.cbSleepDef)
		Config.DEFAULT_CB_SLEEP(cur);
};
					cur.plive = false;
					removeProperties(cur.properties);
					continue;
				}else if(sleeping && cur.plive) {
					 {
	if(cur.cbWake) {
		var cb = alloc.CxAlloc(Callback)();
		cb.type = Callback.PHYSOBJ_WAKE;
		cb.obj = cur;
		callbacks.add(cb);
	}else if(cur.cbWakeDef)
		Config.DEFAULT_CB_WAKE(cur);
};
					addProperties(cur.properties);
				}
				cur.plive = false;
				vec_addeq(cur.f,gravity,cur.gmass);
				if(cur.isBody) {
					var body = cur.body;
					body.t += vec_cross(body.gp,gravity)*body.gmass;
				}
				cur.updateVelocity(dt);
				pre = cxiterator;
			});
		};	
	}
	
	public inline function updatePos(dt:Float) {
		var rem = new CxFastList(PhysObj)(alloc);
		
		CxIter(b,dynamics,{
			b.updatePosition(dt);
			var lost = true;
			CxIter(s,b.shapes,{
				s.updateShape();
				lost = syncShape(s,false)&&lost;
			});
			if(lost) rem.add(b);
		});
		
		CxIter(p,particles,{
			p.updatePosition(dt);
			if(syncParticle(p,false)) rem.add(p);
		});
		
		while(!rem.empty()) {
			var p = rem.front();
			removeObject(p);
			 {
	if(p.cbOutOfBounds) {
		var cb = alloc.CxAlloc(Callback)();
		cb.type = Callback.PHYSOBJ_OUTOFBOUNDS;
		cb.obj = p;
		callbacks.add(cb);
	}else if(p.cbOutOfBoundsDef)
		Config.DEFAULT_CB_OUTOFBOUNDS(p);
};
			rem.pop();
		}
	}
	
	
	
	
	
	

	public inline function prestep(dt:Float) {
		
		
		var pre = null;
		CxIter(c,constraints,{
			var anywake = c.anyBodiesAwake();
			if(c.live && !anywake) {
				c.live = false;
				c.sleep = true;
				cxiterator = constraints.erase(pre,cxiterator);
				continue;
			}
			if(anywake && c.preStep(dt)) {
				cxiterator = constraints.erase(pre,cxiterator);
				c.removeFromBodies();
				c.live = false;
				
				 {
		if(c.cbBreak) {
			var cb = alloc.CxAlloc(Callback)();
			cb.type = Callback.CONSTRAINT_BREAK;
			cb.constraint = c;
			callbacks.add(cb);
		}
	};
				continue;
			}
			c.canSleep = true;
			pre = cxiterator;
		});
	}
	
	
	
	
	
	public inline function iterate_vel(iterations:Int) {
		for(i in 0...iterations) {
			

			var pre = null;
			CxIter(c,constraints,{
				if(c.anyBodiesAwake() && c.applyImpulse()) {
					cxiterator = constraints.erase(pre,cxiterator);
					c.removeFromBodies();
					c.live = false;
					
					 {
		if(c.cbBreak) {
			var cb = alloc.CxAlloc(Callback)();
			cb.type = Callback.CONSTRAINT_BREAK;
			cb.constraint = c;
			callbacks.add(cb);
		}
	};
					continue;
				}
				pre = cxiterator;
			});
		}
	}
	
	public inline function iterate_pos(iterations:Int) {
		for(i in 0...iterations) {
			

			var pre = null;
			CxIter(c,constraints,{
				if(c.anyBodiesAwake() && c.applyImpulse()) {
					cxiterator = constraints.erase(pre,cxiterator);
					c.removeFromBodies();
					c.live = false;
					
					 {
		if(c.cbBreak) {
			var cb = alloc.CxAlloc(Callback)();
			cb.type = Callback.CONSTRAINT_BREAK;
			cb.constraint = c;
			callbacks.add(cb);
		}
	};
					continue;
				}
				pre = cxiterator;
			});
		}
	}
	
	public inline function warmStart() {
		
	}
	
	
	
	public inline function eval_sleep() {
		CxIter(c,constraints,{
			if(c.anyBodiesAwake()) {
				
				
				c.disallowBodySleep();
				c.wakeBodies(this);
				c.sleep = false;
			}
		});
	}
	
	
	
	public function step (dt:Float, ?elastic:Int=Config.DEFAULT_ELASTIC, ?inelastic:Int=Config.DEFAULT_ITERATIONS) {
		if (dt < Const.EPSILON) dt = 0;
		_dt = dt;
		
		if(dt==0) return;
		
		if(register) {
			registerTypes();
			register = false;
		}
		
		callbacks.clear();
		
		if(use_viewport) {
			CxIter(o,visible,{
				o.pvisible = true;
				o.visible = false;
			});
		}
		
		
		
		updateProp(dt);
		
		if(sleeping)
			eval_sleep();
		
		
			
		broadphase();
			
		prestep(dt);
		iterate_vel(elastic);
			
		updateVel_remSleep(dt);

		warmStart();
		iterate_pos(inelastic);
		
		stamp++;
		
		
			
		updatePos(dt);
		
		if(use_viewport) visitate();
	}
	
	
	
	public var cb_Begin:IntHash<Bool>;   public var cb_End:IntHash<Bool>;   public var cb_SenseBegin:IntHash<Bool>;   public var cb_SenseEnd:IntHash<Bool>;   public var cb_PostSolve:IntHash<Bool>;   
	
	public var cb_PreSolve:IntHash<Arbiter->Int>;
		 
	public var cb_PreBegin:IntHash<Arbiter->Int>;
		 
		
	public inline function addGroup(g:Group) {
		if(!groups.has(g)) {
			groups.add(g);
			registerBase(g);
			CxIter(p, g.objs, addObject(p));
		}
	}
	
	public inline function removeGroup(g:Group) {
		if(groups.remove(g)) {
			CxIter(p, g.objs, removeObject(p));
		}
	}
	
	public function registerTypes() {
		     
		 {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_PreSolve.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasPreSolve = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasPreSolve = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_PreBegin.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasPreBegin = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasPreBegin = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_Begin.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasBegin = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasBegin = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_End.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasEnd = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasEnd = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_SenseBegin.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasSenseBegin = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasSenseBegin = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_SenseEnd.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasSenseEnd = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasSenseEnd = tmap.exists(g.cbType));
		} {
			var tmap = new IntHash<Bool>();
			var any = false;
			for(id in cb_PostSolve.keys()) {
				tmap.set(IdRef.fst(id),true);
				tmap.set(IdRef.snd(id),true);
			}
			CxIter(p,objects,p.cbHasPostSolve = tmap.exists(p.cbType));
			CxIter(g,groups, g.cbHasPostSolve = tmap.exists(g.cbType));
		}
	}
	
	public function registerBase(p:Callbackable) {
		     
		 {
			p.cbHasPreSolve = false;
			for(id in cb_PreSolve.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasPreSolve = true;
					break;
				}
			}
		} {
			p.cbHasPreBegin = false;
			for(id in cb_PreBegin.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasPreBegin = true;
					break;
				}
			}
		} {
			p.cbHasBegin = false;
			for(id in cb_Begin.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasBegin = true;
					break;
				}
			}
		} {
			p.cbHasEnd = false;
			for(id in cb_End.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasEnd = true;
					break;
				}
			}
		} {
			p.cbHasSenseBegin = false;
			for(id in cb_SenseBegin.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasSenseBegin = true;
					break;
				}
			}
		} {
			p.cbHasSenseEnd = false;
			for(id in cb_SenseEnd.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasSenseEnd = true;
					break;
				}
			}
		} {
			p.cbHasPostSolve = false;
			for(id in cb_PostSolve.keys()) {
				if(IdRef.fst(id)==p.cbType||IdRef.snd(id)==p.cbType) {
					p.cbHasPostSolve = true;
					break;
				}
			}
		}
	}
	public inline function registerBody(b:Body) {
		registerBase(b);
		CxIterate(b.shapes,registerBase);
	}
	
	
	
	public inline function addCbPreSolve  (a:Int,b:Int,cb:Arbiter->Int)   {
		cb_PreSolve.set(IdRef.pair(a,b),cb);
		register = true;
	}
	public inline function addCbPreBegin  (a:Int,b:Int,cb:Arbiter->Int)   {
		cb_PreBegin.set(IdRef.pair(a,b),cb);
		register = true;
	}
	public inline function addCbBegin     (a:Int,b:Int)                   {
		cb_Begin.set(IdRef.pair(a,b),true);
		register = true;
	}
	public inline function addCbEnd       (a:Int,b:Int)                   {
		cb_End.set(IdRef.pair(a,b),true);
		register = true;
	}
	public inline function addCbSenseBegin(a:Int,b:Int)                   {
		cb_SenseBegin.set(IdRef.pair(a,b),true);
		register = true;
	}
	public inline function addCbSenseEnd  (a:Int,b:Int)                   {
		cb_SenseEnd.set(IdRef.pair(a,b),true);
		register = true;
	}
	public inline function addCbPostSolve (a:Int,b:Int)                   {
		cb_PostSolve.set(IdRef.pair(a,b),true);
		register = true;
	}
	
	
	
	public inline function removeCbPreSolve  (a:Int,b:Int)  {
		if(cb_PreSolve.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbPreBegin  (a:Int,b:Int)  {
		if(cb_PreBegin.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbBegin     (a:Int,b:Int)  {
		if(cb_Begin.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbEnd       (a:Int,b:Int)  {
		if(cb_End.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbSenseBegin(a:Int,b:Int)  {
		if(cb_SenseBegin.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbSenseEnd  (a:Int,b:Int)  {
		if(cb_SenseEnd.remove(IdRef.pair(a,b)))
			register = true;
	}
	public inline function removeCbPostSolve (a:Int,b:Int)  {
		if(cb_PostSolve.remove(IdRef.pair(a,b)))
			register = true;
	}
	
	
	
	
	
	
	

	public inline function ignoreConstraint(b1:PhysObj,b2:PhysObj):Bool {
		var ret = false;
		CxIter(c,b1.p_constraints,{
			if(c.ignore && c.bodyPairExists(b1,b2)) {
				ret = true;
				break;
			}
		});
		return ret;
	}
	
	
	
	
	
	public function broadphase():Void;
	public function visitate():Void;
	
	public function addObject   (o:PhysObj):Void;
	public function removeObject(o:PhysObj):Void;
	
	public function syncShape   (s:Shape,   check:Bool) return false
	public function syncParticle(p:Particle,check:Bool) return false
	
	public function wakeObject(o:PhysObj):Void;
	
	public function rayCast(r:Ray):RayResult return null
	
	public inline function sync(p:PhysObj) {
		if(p.isBody) {
			var b = p.body;
			CxIter(s,b.shapes,syncShape(s,true));
		}else
			syncParticle(p.particle,true);
	}
	
	
	
	
	public function objectAtPoint(x:Float,y:Float):PhysObj return null
	
	
	
	public inline function constrained(p:PhysObj):CxFastList(PhysObj) {
		var ret = new CxFastList(PhysObj)();
		_constrained(p, ret);
		return ret;
	}
	private function _constrained(p:PhysObj, list:CxFastList(PhysObj)) {
		list.add(p);
		CxIter(c, p.p_constraints, {
			var bodies = c.body_list();
			CxIter(b,bodies, if(b!=p && !list.has(b)) _constrained(b, list));
		});
	}
	
	
	
	public inline function touching(p:PhysObj):CxFastList(PhysObj) {
		var ret = new CxFastList(PhysObj)();
		_touching(p, ret);
		return ret;
	}
	
	private function _touching(p:PhysObj, list:CxFastList(PhysObj)) {
		list.add(p);
		
	}
	
	
	
	public inline function impactImpulse(p:PhysObj):Vec2 {
		var jsum = new Vec2();
		
		
		
		return jsum;
	}
	
	public inline function totalImpulsesWithFriction(p:PhysObj):Vec2 {
		var jsum = new Vec2();
		var j    = new Vec2();
		
		
		
		CxIter(c, p.p_constraints,{
			c.impulse(p,j);
			vec_addeq(jsum.p,j.p);
		});
		
		return jsum;
	}
	
	public inline function totalImpulses(p:PhysObj):Vec2 {
		var jsum = new Vec2();
		var j    = new Vec2();
		
		
		
		CxIter(c, p.p_constraints, {
			c.impulse(p,j);
			vec_addeq(jsum.p,j.p);
		});
		
		return jsum;
	}
	
	
	
	public inline function pressure(p:PhysObj):Float {
		var sum = 0.0;
		
		
		var j = new Vec2();
		CxIter(c, p.p_constraints, {
			c.impulse(p,j);
			sum += vec_length(j.p);
		});
		
		return sum;
	}
	
	public inline function crushFactor(p:PhysObj):Float {
		var fsum = 0.0;
		vec_new(jsum,0,0);
		var j    = new Vec2();
		
		
		
		CxIter(c, p.p_constraints, {
			c.impulse(p,j);
			fsum += vec_length(j.p);
			vec_addeq(jsum,j.p);
		});
		
		return fsum-vec_length(jsum);
	}
	
	
	
	public inline function syncMaterials(p:PhysObj) {
		
	}
}














